import rclpy
from rclpy.node import Node
import cv2
import face_recognition


class camera(Node):
    "camera"

    def __init__(self, name):
        super().__init__(name)

    def run(self):
        cap = cv2.VideoCapture(0)
        while True:
            ret, img = cap.read()
            if ret is None:
                break
            # cv2.imshow("IMG", img)
            self.reg(img)
            key = cv2.waitKey(10)
            if key == 27:
                break
        cap.release()
        cv2.destroyAllwindows()

    def reg(self, image):
        face_location = face_recognition.face_locations(
            image, number_of_times_to_upsample=1, model="hog"
        )
        for (
            top,
            right,
            bottom,
            left,
        ) in face_location:
            cv2.rectangle(image, (left, top), (right, bottom), (0, 0, 255), 4)
            self.get_logger().info(f"image center:{(top + bottom)/2},{(left+right)/2}")
        cv2.imshow("object", image)


def main():
    rclpy.init()
    node = camera("test")
    node.run()
    rclpy.spin()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
